#include "PacketModbus.h"
#include "crc16.h"
#include <string.h>
#include "lib.h"

using namespace controller;
using namespace std;

PacketModbus::PacketModbus() {}
PacketModbus::~PacketModbus() {}

PacketBase *PacketModbus::on_get(char *pdata, int size, void *arg)
{
    unsigned char *d = (unsigned char *)&p;
    uint16_t c = 0;
    // 帧超时
    if ((last_frame_ms + PacketModbus::frame_timeout_ms) < LIB::getCurTimestampMs())
    {
        // cout << "帧超时，先复位" << endl;
        reset_recv();
    }
    last_frame_ms = LIB::getCurTimestampMs();
    for (int i = 0; i < size; i++)
    {

        unsigned char data = (unsigned char)pdata[i];

        if (h.len > MODBUS_BUFFER_MAX)
            goto DECODE_ERROR;
        d[h.len] = data;
        h.len++;
        switch (h.state)
        {
        case 0: // address
        case 1: // cmd
            h.state++;
            break;
        case 2: // data
            if (h.len >= 4)
            {
                c = d[h.len - 1];
                c <<= 8;
                c |= d[h.len - 2];
                if (crc16(d, h.len - 2) == c)
                {
                    // 接收到一个数据包
                    h.user_data = (char *)(d + 2);
                    h.user_size = h.len - 4;
                    h.framelen = h.len;
                    h.len = 0;
                    h.state = 0;
                    cout << to_string();
                    return this;
                }
            }
            break;
        default:
            goto DECODE_ERROR;
        }
        continue;
    DECODE_ERROR:
        // 数据异常，转发下一级
        printf("modbus,error:%02X", data);
        h.recv_len = h.len = h.state = 0;
        continue;
    }
    return NULL;
}
int PacketModbus::to_bytes(char *dataout, void **arg)
{
    *arg = NULL;
    if (h.send_size > 0)
    {
        memcpy(dataout, (const void *)&p, h.send_size);
        return h.send_size;
    }
    return 0;
}
string PacketModbus::to_string(string arg)
{
    stringstream ss;
    ss << "modbus,总长:" << h.framelen << " ";
    if (h.framelen >= 4 && h.framelen < (PacketModbus::BUFFER_MAX - 6))
    {

        ss << "addr(" << hex << (int)p.address << ") cmd(" << hex << (int)p.cmd << "),data="

           << ((h.framelen > 4)
                   ? LIB::charArrayToHexString(p.data, h.framelen - 4)
                   : "空")
           << ",crc(" << hex << (int)p.data[h.framelen - 4] << hex << (int)p.data[h.framelen - 3] << ")" << endl;
    }
    else
    {
        ss << "帧异常:" << LIB::charArrayToHexString(&p.address, h.framelen);
    }
    return ss.str();
}

void PacketModbus::reset_recv()
{
    h.reset();
}

int PacketModbus::build(uint8_t address, uint8_t cmd, uint16_t start_reg, uint8_t counter)
{
    uint8_t *d = (uint8_t *)&this->p;
    uint16_t crc = 0;
    p.address = address;
    p.cmd = cmd;
    p.data[0] = start_reg >> 8; // reg 高字节在前
    p.data[1] = start_reg & 0xFF;
    p.data[2] = counter;

    crc = crc16(d, 5);
    p.data[3] = crc & 0XFF; // crc 低字节在前
    p.data[4] = crc >> 8;
    h.send_size = 7;
    h.framelen = h.send_size;
    return h.send_size;
}
